/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "Delaunay.hpp"

#if defined(__cplusplus)
extern "C"
{
#include <libqhull_r/libqhull_r.h>
}
#endif

#include <boost/numeric/conversion/cast.hpp>

using rw::core::ownedPtr;
using namespace rw::math;
using namespace rw::geometry;

namespace {
int build (int dim, double* coords, int nrCoords, std::vector< int >& vertIdxs,
           std::vector< int >& faceIdxs)
{
    vertIdxs.clear ();
    faceIdxs.clear ();

    std::vector< int >& result = vertIdxs;

    int curlong, totlong; /* used !qh_NOmem */
    int exitcode;
    boolT ismalloc = false;
    qhT qh;
    qhT* qhT_pointer = &qh;    // allocate memory for qhull

    char flags[] = "qhull d Qbb Qt Qz";
    // d Qbb: delaunay with scaling of last coordinate (eq. to qdelaunay)
    // Qt: triangulated output
    // Qz: add point-at-infinity (for cocircular points like in rectangles)

    // According to doc. in libqhull_r/user_r.c, the qhull memory should be cleared:
    qh_zero (qhT_pointer, stderr);

    
    // Then the object is constructed.
    exitcode = qh_new_qhull (qhT_pointer, dim, nrCoords, coords, ismalloc, flags, NULL, stderr);
    

    // Loop through all vertices,
    vertexT* vertex;    //, **vertexp;
    for (vertex = qhT_pointer->vertex_list; vertex && vertex->next; vertex = vertex->next) {
        int vertexIdx = qh_pointid (qhT_pointer, vertex->point);
        result.push_back (vertexIdx);
    }

    if (!exitcode) {
        // For all facets:
        for (facetT* facet = qhT_pointer->facet_list; facet && facet->next; facet = facet->next) {
            if (!facet->upperdelaunay) {
                int vertex_n, vertex_i;
                FOREACHvertex_i_ (qhT_pointer, facet->vertices)
                {
                    int vertexIdx = qh_pointid (qhT_pointer, vertex->point);
                    faceIdxs.push_back (vertexIdx);
                }
            }
        }
    }
    if (qhT_pointer->VERIFYoutput && !qhT_pointer->FORCEoutput && !qhT_pointer->STOPpoint &&
        !qhT_pointer->STOPcone)
        qh_check_points (qhT_pointer);

    qh_freeqhull (qhT_pointer, False);
    qh_memfreeshort (qhT_pointer, &curlong, &totlong);
    if (curlong || totlong)
        fprintf (
            stderr,
            "qdelaunay internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
            totlong,
            curlong);
    return exitcode;
}

template< typename S >
typename IndexedTriMeshN0< double, S >::Ptr makeMesh (const std::vector< Vector2D<> >& hullVertices,
                                                      const std::vector< int >& faceIdxs,
                                                      const std::vector< double >& values)
{
    typedef IndexedTriMeshN0< double, S > Mesh;
    typedef typename Mesh::VertexArray VertexArray;
    typedef typename Mesh::TriangleArray TriangleArray;

    // Add vertices
    const rw::core::Ptr< VertexArray > meshV = ownedPtr (new VertexArray (hullVertices.size ()));
    for (size_t i = 0; i < hullVertices.size (); i++) {
        (*meshV)[i] = Vector3D<> (hullVertices[i][0],
                                  hullVertices[i][1],
                                  (values.size () == hullVertices.size ()) ? values[i] : 0);
    }

    // Add triangles
    const rw::core::Ptr< TriangleArray > meshTri =
        ownedPtr (new TriangleArray (faceIdxs.size () / 3));
    for (size_t i = 0; i < faceIdxs.size () / 3; i++) {
        const typename Mesh::tri_type tempTri (
            faceIdxs[i * 3 + 0], faceIdxs[i * 3 + 1], faceIdxs[i * 3 + 2]);
        const Vector3D<>& v1 = (*meshV)[tempTri[0]];
        const Vector3D<>& v2 = (*meshV)[tempTri[1]];
        const Vector3D<>& v3 = (*meshV)[tempTri[2]];

        // Make sure the vertices order is correct according to right-hand rule. Otherwise flip v2
        // and v3.
        const double dotProduct = dot (cross (v2 - v1, v3 - v2), Vector3D<>::z ());
        if (dotProduct < 0.0) {
            (*meshTri)[i] = typename Mesh::tri_type (tempTri[0], tempTri[2], tempTri[1]);
        }
        else {
            (*meshTri)[i] = tempTri;
        }
    }

    const typename Mesh::Ptr mesh = ownedPtr (new Mesh (meshV, meshTri));
    return mesh;
}
}    // namespace

Delaunay::Delaunay ()
{}

Delaunay::~Delaunay ()
{}

IndexedTriMesh<>::Ptr Delaunay::triangulate (const std::vector< Vector2D<> >& vertices,
                                             const std::vector< double >& values)
{
    if (vertices.size () == 3) {
        std::vector< int > faceIdxs (3);
        faceIdxs[0] = 0;
        faceIdxs[1] = 1;
        faceIdxs[2] = 2;
        return makeMesh< uint8_t > (vertices, faceIdxs, values);
    }
    const int nrInputVertices = boost::numeric_cast< int > (vertices.size ());

    // convert the vertice array to an array of double
    double* vertArray = new double[vertices.size () * 2];
    // copy all data into the vertArray
    for (size_t i = 0; i < vertices.size (); i++) {
        const Vector2D<>& v  = vertices[i];
        vertArray[i * 2 + 0] = v[0];
        vertArray[i * 2 + 1] = v[1];
    }
    // Build delaunay triangulation
    std::vector< Vector2D<> > triangVertices;
    std::vector< int > vertiIdxs;
    std::vector< int > faceIdxs;

    
    const int exitcode = build (2, vertArray, nrInputVertices, vertiIdxs, faceIdxs);
    
    delete[] vertArray;
    if (exitcode)
        RW_THROW ("Delaunay triangulation returned with exit code " << exitcode);

    std::vector< int > vertIdxMap (vertices.size ());
    triangVertices.resize (vertiIdxs.size () - 1);    // Qz gives extra vertex
    int triangVerticesI = 0;
    for (size_t i = 0; i < vertiIdxs.size (); i++) {
        if (vertiIdxs[i] < nrInputVertices) {
            triangVertices[triangVerticesI] = vertices[vertiIdxs[i]];
            vertIdxMap[vertiIdxs[i]]        = triangVerticesI;
            triangVerticesI++;
        }
    }
    for (size_t i = 0; i < faceIdxs.size (); i++) {
        const int tmp = faceIdxs[i];
        faceIdxs[i]   = vertIdxMap[tmp];
    }

    // Create the mesh
    if (vertices.size () <= 256) {
        return makeMesh< uint8_t > (triangVertices, faceIdxs, values);
    }
    else if (vertices.size () <= 65536) {
        return makeMesh< uint16_t > (triangVertices, faceIdxs, values);
    }
    else if (vertices.size () <= 4294967296) {
        return makeMesh< uint32_t > (triangVertices, faceIdxs, values);
    }
    else {
        return makeMesh< uint64_t > (triangVertices, faceIdxs, values);
    }
}
